#include <iostream>
#include <vector>
#include <fstream>
#include <string>


#include "cameracalibrator.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

CameraCalibrator::CameraCalibrator()
{
    flag=0;
    mustInitUndistort = true;
}


CameraCalibrator::~CameraCalibrator()
{
}

int CameraCalibrator::addChessboardPoints(cv::Size &board_size, cv::Mat imageInput)
{
    std::vector< cv::Point2f> imageCorners;  //对应图像上(i, j)棋盘格角点
    std::vector<std::vector<cv::Point2f >> image_points_seq; /* 保存检测到的所有角点 */
    std::vector< cv::Point3f> objectCorners;  //对应世界坐标系(X,Y,Z)棋盘格角点

    /* 提取角点 */
    if (0 == cv::findChessboardCorners(imageInput,board_size,imageCorners))
    {
        std::cout << "can not find chessboard corners!\n" << std::endl; //找不到角点
        exit(1);
    }
    else
    {
        cv::Mat view_gray;
        cv::cvtColor(imageInput,view_gray,CV_RGB2GRAY);
        /* 亚像素精确化 */
        cv::find4QuadCornerSubpix(view_gray,imageCorners,cv::Size(11,11)); //对粗提取的角点进行精确化
        image_points_seq.push_back(imageCorners);  //保存亚像素角点
        /* 在图像上显示角点位置 */
        cv::drawChessboardCorners(view_gray,board_size,imageCorners,true); //用于在图片中标记角点
        cv::imshow("Camera Calibration",view_gray);//显示图片
        cv::waitKey(500);//暂停0.1S
    }
    imagePoints.push_back(imageCorners);
    objectPoints.push_back(objectCorners);
}

double CameraCalibrator::calibrate(cv::Size &image_size)
{
    mustInitUndistort = true;  //必须重新去畸变
    std::vector<cv::Mat> rvecs, tvecs;  //输出旋转向量和平移向量
    return  calibrateCamera(objectPoints,imagePoints,image_size,cameraMatrix,distCoeffs,rvecs,tvecs,flag);
}

cv::Mat CameraCalibrator::remap(cv::Mat &imageInput, cv::Size &image_size)
{
    std::cout << "aaa" << std::endl;
    cv::Mat undistorted; //未畸变图像

    if (mustInitUndistort)
    {
        std::cout << "mmm" << std::endl;
        //初始化未畸变矫正映射图像(相机矩阵，畸变矩阵，可选矫正矩阵，生成的未畸变相机矩阵，生成的未畸变相机矩阵的尺寸，输出的映射图像类型，X坐标映射函数，Y坐标映射函数)
        cv::initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
        std::cout << "nnn" << std::endl;
        mustInitUndistort = false;
        std::cout << "vvv" << std::endl;
    }
    std::cout << "bbb" << std::endl;
    cv::remap(imageInput, undistorted, mapx, mapy,cv::INTER_LINEAR);  //应用映射函数，输出未畸变图像  | 差值类型
    std::cout << "ccc" << std::endl;
    return undistorted;
}
